#include "DynamicSimulator.h"
#include <stdio.h>
#include <mkl_lapacke.h>  
#include <mkl.h>  

#pragma comment(lib,"mkl_lapack95_lp64.lib")
#pragma comment(lib,"mkl_rt.lib")

DynamicSimulator::DynamicSimulator(void)
{
	simplex = new ElementContainer();
}

DynamicSimulator::~DynamicSimulator(void)
{
}

void DynamicSimulator::LoadVolumetricMesh( char* name )
{
	loader.LoadTetMesh(simplex, name);

	f0 = (float*)malloc(simplex->vertNum*3*sizeof(float));
	xi = (float*)malloc(simplex->vertNum*3*sizeof(float));
	f = (float*)malloc(simplex->vertNum*3*sizeof(float));
	K = (float**)malloc(simplex->vertNum*3*sizeof(float*));

	for (int i=0;i<simplex->vertNum*3;i++)
	{
		K[i] = (float*)malloc(simplex->vertNum*3*sizeof(float));
		f0[i] = 0;

		for (int j=0;j<simplex->vertNum*3;j++)
		{
			K[i][j] = 0;
		}
	}

	printf("Vertex\n");

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		printf("%f %f %f\n",vert->position[0],vert->position[1],vert->position[2]);
	}

	printf("Tet\n");

	for (int i=0;i<simplex->tetNum;i++)
	{
		Tet* t = simplex->tets.at(i);

		printf("%d %d %d %d\n",t->vertices[0]->index,t->vertices[1]->index,t->vertices[2]->index,t->vertices[3]->index);
	}
}

void DynamicSimulator::InitElement( float E, float mu )
{
	this->E = E;
	this->mu = mu;
	solver = new Integrator;
	solver->InitIntegrator(simplex->vertNum);

	InitPinv();
	InitMe();
	InitKe();
}

void DynamicSimulator::InitKe()
{
	float a[4], b[4], c[4], d[4];
	float Be[6][12];
	float De[6][6];
	float Inter[12][6];
	float sum;

	for (int i=0;i<6;i++)
	{
		for (int j=0;j<12;j++)
		{
			Be[i][j] = 0;
		}
	}

	for (int i=0;i<6;i++)
	{
		for (int j=0;j<6;j++)
		{
			De[i][j] = 0;
		}
	}

	// De

	float multiplier = E / ((1 + mu) * (1 - 2 * mu));

	for (int i=0;i<3;i++)
	{
		for (int j=0;j<3;j++)
		{
			if (i==j)
			{
				De[i][j] = (1 - mu) * multiplier;
			}else{
				De[i][j] = mu * multiplier;
			}
		}
	}

	De[3][3] = De[4][4] = De[5][5] = (1 - 2 * mu) * multiplier / 2; //E*((1-2*NU)/(2*((1+NU)*(1-2*NU))));

	/*for (int i=0;i<6;i++)
	{
		for (int j=0;j<6;j++)
		{
			printf("%f ",De[i][j]);
		}
		printf("\n");
	}*/


	for (int ti=0;ti<simplex->tetNum;ti++)
	{
		Tet* t = simplex->tets.at(ti);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		/*va = new Vertex;
		vb = new Vertex;
		vc = new Vertex;
		vd = new Vertex;

		va->position = vec3f(3,6,-2);
		vb->position = vec3f(9,-13,4);
		vc->position = vec3f(5,11,-7);
		vd->position = vec3f(-1,-8,41);*/


		// Be
		CalDeterminant(a[0],b[0],c[0],d[0],vb,vc,vd);
		CalDeterminant(a[1],b[1],c[1],d[1],vc,vd,va);
		CalDeterminant(a[2],b[2],c[2],d[2],vd,va,vb);
		CalDeterminant(a[3],b[3],c[3],d[3],va,vb,vc);

		Be[0][0] = b[0];
		Be[0][3] = -b[1];
		Be[0][6] = b[2];
		Be[0][9] = -b[3];

		Be[1][1] = c[0];
		Be[1][4] = -c[1];
		Be[1][7] = c[2];
		Be[1][10] = -c[3];

		Be[2][2] = d[0];
		Be[2][5] = -d[1];
		Be[2][8] = d[2];
		Be[2][11] = -d[3];

		Be[3][0] = c[0];
		Be[3][3] = -c[1];
		Be[3][6] = c[2];
		Be[3][9] = -c[3];

		Be[3][1] = b[0];
		Be[3][4] = -b[1];
		Be[3][7] = b[2];
		Be[3][10] = -b[3];

		Be[4][2] = c[0];
		Be[4][5] = -c[1];
		Be[4][8] = c[2];
		Be[4][11] = -c[3];

		Be[4][1] = d[0];
		Be[4][4] = -d[1];
		Be[4][7] = d[2];
		Be[4][10] = -d[3];

		Be[5][0] = d[0];
		Be[5][3] = -d[1];
		Be[5][6] = d[2];
		Be[5][9] = -d[3];

		Be[5][2] = b[0];
		Be[5][5] = -b[1];
		Be[5][8] = b[2];
		Be[5][11] = -b[3];

		for (int i=0;i<6;i++)
		{
			for (int j=0;j<12;j++)
			{
				Be[i][j] *= (1 / (6 * t->volume));
			}
		}

		/*for (int i=0;i<6;i++)
		{
			for (int j=0;j<12;j++)
			{
				printf("%.02f ",Be[i][j]);
			}
			printf("\n");
		}*/

		// Ke = Ve * Be~ * De * Be

		// Inter = Be~ * De

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<6;j++)
			{
				sum = 0;

				for (int k=0;k<6;k++)
				{
					sum += Be[k][i] * De[k][j];
				}

				Inter[i][j] = sum;
			}
		}

		// Ke = Ve * Inter * Be

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				sum = 0;

				for (int k=0;k<6;k++)
				{
					sum += Inter[i][k] * Be[k][j];
				}

				t->Ke[i][j] = sum * t->volume;// / (t->volume * 36);
			}
		}

		//printf("\n\n");

		/*for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				printf("%f ",t->Ke[i][j]);
			}
			printf("\n");
		}*/
		
	}
}

void DynamicSimulator::InitMe()
{
	float mass;

	/*for (int i=0;i<simplex->tetNum;i++)
	{
		Tet* t = simplex->tets.at(i);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		t->volume = abs((va->origin - vd->origin) * ((vb->origin - vd->origin) ^ (vc->origin - vd->origin)));
		t->volume /= 6.0;

		mass = DENSITY * 0.25 * t->volume;

		va->mass += mass;
		vb->mass += mass;
		vc->mass += mass;
		vd->mass += mass;
	}*/

	for (int i=0;i<simplex->tetNum;i++)
	{
		Tet* t = simplex->tets.at(i);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		/*va = new Vertex;
		vb = new Vertex;
		vc = new Vertex;
		vd = new Vertex;

		va->origin = vec3f(3,6,-2);
		vb->origin = vec3f(9,-13,4);
		vc->origin = vec3f(5,11,-7);
		vd->origin = vec3f(-1,-8,41);*/

		t->volume = abs((va->origin - vd->origin) * ((vb->origin - vd->origin) ^ (vc->origin - vd->origin)));
		t->volume /= 6.0;
	}

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		vert->mass = 1000000;
	}

}

void DynamicSimulator::CalDeterminant( float& a, float& b, float& c, float& d, Vertex* va, Vertex* vb, Vertex* vc )
{
	a = va->position[1] * vb->position[2] * vc->position[0] + va->position[2] * vb->position[0] * vc->position[1] + va->position[0] * vb->position[1] * vc->position[2] - (va->position[2] * vb->position[1] * vc->position[0] + va->position[0] * vb->position[2] * vc->position[1] + va->position[1] * vb->position[0] * vc->position[2]);

	b = va->position[1] * vb->position[2] + va->position[2] * vc->position[1] + vb->position[1] * vc->position[2] - (va->position[2] * vb->position[1] + va->position[1] * vc->position[2] + vb->position[2] * vc->position[1]);
	b *= -1;

	c = va->position[0] * vb->position[2] + va->position[2] * vc->position[0] + vb->position[0] * vc->position[2] - (va->position[2] * vb->position[0] + va->position[0] * vc->position[2] + vb->position[2] * vc->position[0]);

	d = va->position[1] * vb->position[0] + va->position[0] * vc->position[1] + vb->position[1] * vc->position[0] - (va->position[0] * vb->position[1] + va->position[1] * vc->position[0] + vb->position[0] * vc->position[1]);
}

void DynamicSimulator::CalForce()
{
	float force[12];
	float x[12];
	float x0[12];
	float Re[12][12];
	float sub[12][12];
	float ReInv[12][12];
	float sig[12][12];
	float sum, sum_adj;

	mat3f ReI;

	static int frame = 0;

	frame++;

	if (frame<2)
	{
		Perturb();
	}

	CalRe();

	for (int i=0;i<12;i++)
	{
		for (int j=0;j<12;j++)
		{
			Re[i][j] = 0;
			ReInv[i][j] = 0;
		}
	}

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		vert->force = vec3f(0,0,0);
	}

	for (int ti=0;ti<simplex->tetNum;ti++)
	{
		Tet* t = simplex->tets.at(ti);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		x[0] = va->position[0];
		x[1] = va->position[1];
		x[2] = va->position[2];

		x[3] = vb->position[0];
		x[4] = vb->position[1];
		x[5] = vb->position[2];

		x[6] = vc->position[0];
		x[7] = vc->position[1];
		x[8] = vc->position[2];

		x[9] = vd->position[0];
		x[10] = vd->position[1];
		x[11] = vd->position[2];

		x0[0] = va->origin[0];
		x0[1] = va->origin[1];
		x0[2] = va->origin[2];

		x0[3] = vb->origin[0];
		x0[4] = vb->origin[1];
		x0[5] = vb->origin[2];

		x0[6] = vc->origin[0];
		x0[7] = vc->origin[1];
		x0[8] = vc->origin[2];

		x0[9] = vd->origin[0];
		x0[10] = vd->origin[1];
		x0[11] = vd->origin[2];

		ReI = t->Re;

		float checkO = ReI.col(0) * ReI.col(1);
		float check1 = ReI.col(1) * ReI.col(2);
		float check2 = ReI.col(0) * ReI.col(2);
		float check3 = ReI.col(0) * ReI.col(0);
		float check4 = ReI.col(1) * ReI.col(1);
		float check5 = ReI.col(2) * ReI.col(2);
		float check6 = ReI.Determinant();

		ReI = ReI.Inverse();

		//ReI = mat3f(1,0,0,0,1,0,0,0,1);


		for (int i=0;i<3;i++)
		{
			for (int j=0;j<3;j++)
			{
				Re[i][j] = Re[3+i][3+j] = Re[6+i][6+j] = Re[9+i][9+j] = t->Re[i][j];
				ReInv[i][j] = ReInv[3+i][3+j] = ReInv[6+i][6+j] = ReInv[9+i][9+j] = ReI[i][j];
			}
		}

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				sum = 0;

				for (int k=0;k<12;k++)
				{
					sum += Re[i][k] * t->Ke[k][j];
				}

				sub[i][j] = sum;
			}
		}

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				sum = 0;

				for (int k=0;k<12;k++)
				{
					sum += sub[i][k] * ReInv[k][j];
				}

				sig[i][j] = sum;
			}
		}

		for (int i=0;i<12;i++)
		{
			sum = 0;
			sum_adj = 0;

			for (int j=0;j<12;j++)
			{
				sum += sig[i][j] * x[j];
				sum_adj += sub[i][j] * x0[j];
			}

			force[i] = sum - sum_adj;
		}

		va->force[0] += force[0];
		va->force[1] += force[1];
		va->force[2] += force[2];

		vb->force[0] += force[3];
		vb->force[1] += force[4];
		vb->force[2] += force[5];

		vc->force[0] += force[6];
		vc->force[1] += force[7];
		vc->force[2] += force[8];

		vd->force[0] += force[9];
		vd->force[1] += force[10];
		vd->force[2] += force[11];

		/*for (int i=0;i<3;i++)
		{
			va->velocity[i] += 0.1 * va->force[i] / va->mass;
			va->position[i] += 0.1 * va->velocity[i];

			vb->velocity[i] += 0.1 * vb->force[i] / vb->mass;
			vb->position[i] += 0.1 * vb->velocity[i];

			vc->velocity[i] += 0.1 * vc->force[i] / vc->mass;
			vc->position[i] += 0.1 * vc->velocity[i];

			vd->velocity[i] += 0.1 * vd->force[i] / vd->mass;
			vd->position[i] += 0.1 * vd->velocity[i];
		}*/

	}

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		vert->velocity += ((vert->force / vert->mass) * 0.1);
		vert->position += vert->velocity * 0.1;
	}

	//Vertex* va = simplex->vertices.at(4);
	Vertex* vb = simplex->vertices.at(0);
	Vertex* vc = simplex->vertices.at(1);
	Vertex* vd = simplex->vertices.at(2);

	//va->velocity = vec3f(0,0,0);
	vb->velocity = vec3f(0,0,0);
	vc->velocity = vec3f(0,0,0);
	vd->velocity = vec3f(0,0,0);

	//va->position = va->origin;
	vb->position = vb->origin;
	vc->position = vc->origin;
	vd->position = vd->origin;

}

void DynamicSimulator::Perturb()
{
	vec3f Offset(100000,200000,200000);

	Vertex* a = simplex->vertices.at(157);
	Vertex* b = simplex->vertices.at(176);
	Vertex* c = simplex->vertices.at(177);
	Vertex* d = simplex->vertices.at(179);
	Vertex* e = simplex->vertices.at(195);
	Vertex* f = simplex->vertices.at(196);
	Vertex* g = simplex->vertices.at(197);
	Vertex* h = simplex->vertices.at(198);
	Vertex* i = simplex->vertices.at(323);

	a->fext = Offset;
	b->fext = Offset;
	c->fext = Offset;
	d->fext = Offset;
	e->fext = Offset;
	f->fext = Offset;
	g->fext = Offset;
	h->fext = Offset;
	i->fext = Offset;

	/*v->velocity = vec3f(0,0,0);

	v->position += vec3f(0.1,0.2,0.2);

	for (int i=0;i<3;i++)
	{
		printf("%f ",v->force[i]);
	}*/

	printf("\n");

}

void DynamicSimulator::InitPinv()
{
	mat4f P;

	for (int ti=0;ti<simplex->tetNum;ti++)
	{
		Tet* t = simplex->tets.at(ti);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		for (int i=0;i<3;i++)
		{
			P[i][0] = va->position[i];
			P[i][1] = vb->position[i];
			P[i][2] = vc->position[i];
			P[i][3] = vd->position[i];
		}

		for (int i=0;i<4;i++)
		{
			P[3][i] = 1.0;
		}

		t->Pinv = P.Inverse();
	}
}

void DynamicSimulator::CalRe()
{
	mat4f Q;
	mat4f A;
	mat3f B;

	for (int ti=0;ti<simplex->tetNum;ti++)
	{
		Tet* t = simplex->tets.at(ti);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		for (int i=0;i<3;i++)
		{
			Q[i][0] = va->position[i];
			Q[i][1] = vb->position[i];
			Q[i][2] = vc->position[i];
			Q[i][3] = vd->position[i];
		}

		for (int i=0;i<4;i++)
		{
			Q[3][i] = 1.0;
		}

		A = Q * t->Pinv;

		for (int i=0;i<3;i++)
		{
			for (int j=0;j<3;j++)
			{
				B[i][j] = A[i][j];
			}
		}

		t->Re = PolarDecomposition(B);
		//t->Re = QRDecomposition(B);
	}
}

mat3f DynamicSimulator::PolarDecomposition( mat3f B )
{
	mat3f R;

	double A[9];
	double superb[9];
	double s[3];
	double u[9];
	double v[9];	

	lapack_int m = 3;
	lapack_int n = 3;
	lapack_int ldu = 3;
	lapack_int ldvt = 3;
	lapack_int lda = 3; 

	float sum;

	for (int i=0;i<3;i++)
	{
		for (int j=0;j<3;j++)
		{
			A[i*3+j] = B[i][j];
		}
	}

	LAPACKE_dgesvd( LAPACK_ROW_MAJOR, 'A', 'A', m, n, A, lda, s, u, ldu, v, ldvt, superb );

	for (int i=0;i<3;i++)
	{
		for (int j=0;j<3;j++)
		{
			sum = 0;

			for (int k=0;k<3;k++)
			{
				sum += u[i*3+k] * v[k*3+j];
			}

			R[i][j] = sum;
		}
	}

	return R;
}

void DynamicSimulator::AssemblyK()
{
	int scale = simplex->vertNum * 3;
	float force[12];
	float x[12];
	float x0[12];
	float Re[12][12];
	float sub[12][12];
	float ReInv[12][12];
	float sig[12][12];
	float sum;

	mat3f ReI;

	CalRe();

	for (int i=0;i<12;i++)
	{
		for (int j=0;j<12;j++)
		{
			Re[i][j] = 0;
			ReInv[i][j] = 0;
		}
	}

	for (int i=0;i<scale;i++)
	{
		f0[i] = 0;
	}

	for (int i=0;i<scale;i++)
	{
		for (int j=0;j<scale;j++)
		{
			this->K[i][j] = 0;
		}
	}

	for (int ti=0;ti<simplex->tetNum;ti++)
	{
		Tet* t = simplex->tets.at(ti);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		x[0] = va->position[0];
		x[1] = va->position[1];
		x[2] = va->position[2];

		x[3] = vb->position[0];
		x[4] = vb->position[1];
		x[5] = vb->position[2];

		x[6] = vc->position[0];
		x[7] = vc->position[1];
		x[8] = vc->position[2];

		x[9] = vd->position[0];
		x[10] = vd->position[1];
		x[11] = vd->position[2];

		x0[0] = va->origin[0];
		x0[1] = va->origin[1];
		x0[2] = va->origin[2];

		x0[3] = vb->origin[0];
		x0[4] = vb->origin[1];
		x0[5] = vb->origin[2];

		x0[6] = vc->origin[0];
		x0[7] = vc->origin[1];
		x0[8] = vc->origin[2];

		x0[9] = vd->origin[0];
		x0[10] = vd->origin[1];
		x0[11] = vd->origin[2];

		ReI = t->Re;

		ReI = ReI.Inverse();

		for (int i=0;i<3;i++)
		{
			for (int j=0;j<3;j++)
			{
				Re[i][j] = Re[3+i][3+j] = Re[6+i][6+j] = Re[9+i][9+j] = t->Re[i][j];
				ReInv[i][j] = ReInv[3+i][3+j] = ReInv[6+i][6+j] = ReInv[9+i][9+j] = ReI[i][j];
			}
		}

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				sum = 0;

				for (int k=0;k<12;k++)
				{
					sum += Re[i][k] * t->Ke[k][j];
				}

				sub[i][j] = sum;
			}
		}

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				sum = 0;

				for (int k=0;k<12;k++)
				{
					sum += sub[i][k] * ReInv[k][j];
				}

				sig[i][j] = sum;
			}
		}

		for (int i=0;i<12;i++)
		{
			sum = 0;

			for (int j=0;j<12;j++)
			{
				sum += sub[i][j] * x0[j];
			}

			force[i] = sum;
		}


		for (int i=0;i<3;i++)
		{
			this->f0[va->index*3+i] += force[i];
			this->f0[vb->index*3+i] += force[3+i];
			this->f0[vc->index*3+i] += force[6+i];
			this->f0[vd->index*3+i] += force[9+i];
		}

		/*for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				printf("%.02f ",sig[i][j]);
			}
			printf("\n");
		}

		printf("\n\n");*/

		for (int i=0;i<3;i++)
		{
			for (int j=0;j<3;j++)
			{
				// fa va
				this->K[va->index*3+i][va->index*3+j] += sig[i][j];
				// fa vb
				this->K[va->index*3+i][vb->index*3+j] += sig[i][3+j];
				// fa vc
				this->K[va->index*3+i][vc->index*3+j] += sig[i][6+j];
				// fa vd
				this->K[va->index*3+i][vd->index*3+j] += sig[i][9+j];
				// fb va
				this->K[vb->index*3+i][va->index*3+j] += sig[3+i][j];
				// fb vb
				this->K[vb->index*3+i][vb->index*3+j] += sig[3+i][3+j];
				// fb vc
				this->K[vb->index*3+i][vc->index*3+j] += sig[3+i][6+j];
				// fb vd
				this->K[vb->index*3+i][vd->index*3+j] += sig[3+i][9+j];
				// fc va
				this->K[vc->index*3+i][va->index*3+j] += sig[6+i][j];
				// fc vb
				this->K[vc->index*3+i][vb->index*3+j] += sig[6+i][3+j];
				// fc vc
				this->K[vc->index*3+i][vc->index*3+j] += sig[6+i][6+j];
				// fc vd
				this->K[vc->index*3+i][vd->index*3+j] += sig[6+i][9+j];
				// fd va
				this->K[vd->index*3+i][va->index*3+j] += sig[9+i][j];
				// fd vb
				this->K[vd->index*3+i][vb->index*3+j] += sig[9+i][3+j];
				// fd vc
				this->K[vd->index*3+i][vc->index*3+j] += sig[9+i][6+j];
				// fd vd
				this->K[vd->index*3+i][vd->index*3+j] += sig[9+i][9+j];
			}
		}

	}

	/*for (int i=0;i<scale;i++)
	{
		for (int j=0;j<scale;j++)
		{
			printf("%.02f ",this->K[i][j]);
		}
		printf("\n");
	}

	printf("\n");*/
	

}

void DynamicSimulator::Update( float dt )
{
	timestep = dt;

	AssemblyK();

	//ForwardEuler();
	BackwardEuler();

	//Vertex* va = simplex->vertices.at(4);
	Vertex* vb = simplex->vertices.at(0);
	Vertex* vc = simplex->vertices.at(1);
	Vertex* vd = simplex->vertices.at(2);

	//va->velocity = vec3f(0,0,0);
	vb->velocity = vec3f(0,0,0);
	vc->velocity = vec3f(0,0,0);
	vd->velocity = vec3f(0,0,0);

	//va->position = va->origin;
	vb->position = vb->origin;
	vc->position = vc->origin;
	vd->position = vd->origin;
}

void DynamicSimulator::ForwardEuler()
{
	int scale = simplex->vertNum * 3;
	float sum;

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		xi[i*3+0] = vert->position[0];
		xi[i*3+1] = vert->position[1];
		xi[i*3+2] = vert->position[2];
	}

	for (int i=0;i<scale;i++)
	{
		sum = 0;

		for (int j=0;j<scale;j++)
		{
			sum += K[i][j] * xi[j];
		}

		f[i] = sum - f0[i];
	}

	static int frame = 0;

	frame++;

	if (frame>10)
	{
		//return;
	}

	//printf("%d\n",frame);

	/*for (int i=0;i<scale;i++)
	{
		printf("%f\n",f[i]);
	}*/

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		vert->velocity[0] += timestep * (vert->fext[0] - f[i*3+0]) / vert->mass;
		vert->position[0] += timestep * vert->velocity[0];

		vert->velocity[1] += timestep * (vert->fext[1] - f[i*3+1]) / vert->mass;
		vert->position[1] += timestep * vert->velocity[1];

		vert->velocity[2] += timestep * (vert->fext[2] - f[i*3+2]) / vert->mass;
		vert->position[2] += timestep * vert->velocity[2];

		vert->fext = vec3f(0,0,0);

		//printf("%f %f %f\n",vert->velocity[0],vert->velocity[1],vert->velocity[2]);
	}

	/*for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		vert->velocity *= 0.9;
	}*/


}

mat3f DynamicSimulator::QRDecomposition( mat3f B )
{
	mat3f R;
	vec3f cache;

	cache = B.col(0).GetNormalized();

	R[0][0] = cache[0];
	R[1][0] = cache[1];
	R[2][0] = cache[2];

	cache = B.col(1) - R.col(0) * (B.col(1) * R.col(0));

	cache = cache.GetNormalized();

	R[0][1] = cache[0];
	R[1][1] = cache[1];
	R[2][1] = cache[2];

	cache = (R.col(0) ^ R.col(1)).GetNormalized();

	R[0][2] = cache[0];
	R[1][2] = cache[1];
	R[2][2] = cache[2];

	return R;

}

void DynamicSimulator::BackwardEuler()
{
	int scale = simplex->vertNum * 3;
	int index;
	Vertex* vert;
	float sum;
	int offset;

	for (int i=0;i<scale;i++)
	{
		for (int j=0;j<scale;j++)
		{
			solver->check_A[i][j] = K[i][j] * timestep * timestep;
			
			if (i==j)
			{
				index = i / 3;
				vert = simplex->vertices.at(index);

				solver->check_A[i][j] += vert->mass;
			}
		}
	}

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		xi[i*3+0] = vert->position[0];
		xi[i*3+1] = vert->position[1];
		xi[i*3+2] = vert->position[2];
	}

	for (int i=0;i<scale;i++)
	{
		sum = 0;

		for (int j=0;j<scale;j++)
		{
			sum += K[i][j] * xi[j];
		}

		f[i] = sum;
	}

	for (int i=0;i<scale;i++)
	{
		index = i / 3;
		vert = simplex->vertices.at(index);
		offset = i - 3 * index;

		solver->check_b[i] = timestep * (vert->fext[offset] + f0[i] - f[i]) + vert->mass * vert->velocity[offset];
	}

	solver->SparseSolver();

	for (int i=0;i<simplex->vertNum;i++)
	{
		vert = simplex->vertices.at(i);

		vert->velocity[0] = solver->check_x[i*3+0];
		vert->velocity[1] = solver->check_x[i*3+1];
		vert->velocity[2] = solver->check_x[i*3+2];

		vert->position += timestep * vert->velocity;

		vert->fext = vec3f(0,0,0);
	}
}